//
// Created by shuoy on 8/10/21.
//

#ifndef VILO_A1KINEMATICS_H
#define VILO_A1KINEMATICS_H

#include <eigen3/Eigen/Dense>
#include "../utils/parameters.h"

class A1Kinematics
{

public:
    A1Kinematics() = default;
    ~A1Kinematics() = default;

    // rho opt are contact offset cz
    // rho fix are body offset x& y, thigh offset, upper leg length, lower leg length
    // functions with eigen interface
    // forward kinematics 3x1
    Eigen::Vector3d fk(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix);
    // jacobian   3x3
    Eigen::Matrix3d jac(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix);
    // the partial derivative of fk wrt rho opt   3xRHO_OPT_SIZE
    Eigen::Matrix<double, 3, RHO_OPT_SIZE> dfk_drho(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix);
    // the partial derivative of jacobian wrt q    9x3
    Eigen::Matrix<double, 9, 3> dJ_dq(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix);
    // the partial derivative of jacobian wrt rho opt   9xRHO_OPT_SIZE
    Eigen::Matrix<double, 9, RHO_OPT_SIZE> dJ_drho(Eigen::Vector3d q, Eigen::VectorXd rho_opt, Eigen::VectorXd rho_fix);

private:
    // functions with basic C++ interface, generated by Matlab
    void autoFunc_fk_pf_pos(const double in1[3], const double in2[RHO_OPT_SIZE], const double in3[RHO_FIX_SIZE], double p_bf[3]);
    void autoFunc_d_fk_dt(const double in1[3], const double in2[RHO_OPT_SIZE], const double in3[RHO_FIX_SIZE], double jacobian[9]);
    void autoFunc_d_fk_drho(const double in1[3], const double in2[RHO_OPT_SIZE], const double in3[RHO_FIX_SIZE], double d_fk_drho[D_FK_DRHO_SIZE]);
    void autoFunc_dJ_dt(const double in1[3], const double in2[RHO_OPT_SIZE], const double in3[RHO_FIX_SIZE], double dJ_dq[27]);
    void autoFunc_dJ_drho(const double in1[3], const double in2[RHO_OPT_SIZE], const double in3[RHO_FIX_SIZE], double dJ_drho[D_J_DRHO_SIZE]);
};

#endif // VILO_A1KINEMATICS_H
